/******************************************************************************
* AIR SYSTEMS LABORATORY
* AEROSPACE ENGINEERING
* UNIVERISTY OF TEXAS AT AUSTIN
* UAV FLIGHT SOFTWARE 
* DESCRIPTION:
*   
*
* SOURCE: 
* Orignial: 08/14/2008  Jorge Alvarez joroge@gmail.com
* Modified: 03/20/2009	Sergio García-Nieto sergarro@isa.upv.es
* $Id: gps.c 4 2009-07-21 17:12:11Z joroge@gmail.com $
*************************************************************************************/
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <math.h>
#include <pthread.h>

#include "serial.h"
#include "globaldefs.h"
#include "utilities.h"
#include "gps.h"

void gpsRequest(int sPort)
{
	int 		nbytes=0;
	static byte	packet1[8]={SYNCHAR1,SYNCHAR2,NAV,POSLLH,0x00,0x00,0x03,0x0a};
	static byte	packet2[8]={SYNCHAR1,SYNCHAR2,NAV,VELNED,0x00,0x00,0x13,0x3a};

  /*********************************************************************
   *Write Position Command
   *********************************************************************/
    nbytes = write(sPort,(char*)&packet1,8); 
  /*********************************************************************
   *Send Velocity Command
  *********************************************************************/
    nbytes = write(sPort,(char*)&packet2,8);
 
}

void gpsAcquisition(int sPort,struct gps *gpspacket)
{    
	int 		nbytes1=0;
	int 		nbytes2=0;
	byte  		input_position[POSITION_PACKET_LENGTH]={0,},input_velocity[VELOCITY_PACKET_LENGTH]={0,};

  /*********************************************************************
   *Read Position
  *********************************************************************/	
	//nbytes = tcischars(sPort0);
    //printf("Tcischar DELTAS function %d\n",nbytes);
    nbytes1=read(sPort,input_position,POSITION_PACKET_LENGTH);
    //printf("[GPS]: POSITION BYTES READ %d\n",nbytes);

  /*********************************************************************
   *Read Velocity
  *********************************************************************/
    //nbytes = tcischars(sPort0);
    //printf("Tcischar RotAcelT function %d\n",nbytes);
    nbytes2=read(sPort,input_velocity,VELOCITY_PACKET_LENGTH);
    //printf("[GPS]: VELOCITY BYTES READ %d\n",nbytes);

  /***************************************************************************************
   *Decode Position packet
  ***************************************************************************************/
  if(nbytes1>0)
  {	  
  if(checksumCheckGPS(input_position,POSITION_PACKET_LENGTH)==TRUE)
		 {
			//printf("[GPS]:Nice,it worked position..!\n");
			/*  position LLA degreas and feet for altitude MSL */ 
			gpspacket->lon=(float)(longFromBytesLE(&input_position[10])*1.0e-7); 
			gpspacket->lat=(float)(longFromBytesLE(&input_position[14])*1.0e-7); 
			gpspacket->alt_msl=(float)(longFromBytesLE(&input_position[18])*1.0e-3);//meters
			//data->alt_msl=(double)(longFromBytesLE(&buffer[18])*(3.2808399/1000)); //feet
		   }
		  else 
          {
           	//printf("[GPS]:Checksum error.!\n");
            	//imupacket.error_type = CHECKSUM_ERROR; 
          }
  }       
  if(nbytes2>0)
  {  
  if(checksumCheckGPS(input_velocity,VELOCITY_PACKET_LENGTH)==TRUE)
		  {
			/*  velocity NED in knots */ 
			//data->vn=(double)(longFromBytesLE(&buffer[10])*(3600/185200)); // knots
			gpspacket->vn=(float)(longFromBytesLE(&input_velocity[10])*1.0e-2); // m/s
			gpspacket->ve=(float)(longFromBytesLE(&input_velocity[14])*1.0e-2); 
			gpspacket->vd=(float)(longFromBytesLE(&input_velocity[18])*1.0e-2);
		   }
		  else 
           {
           	//printf("[GPS]:Checksum error.!\n");
           	//imupacket.error_type = CHECKSUM_ERROR; 
           	}
	}
} /* end void *GPSadquisition*/


/***************************************************************************************
 *check the checksum of the GPS packet
 ***************************************************************************************/
int checksumCheckGPS(byte* buffer, int packet_len)
{
   short     	 i=0;
   byte			 ck_a=0,ck_b=0;
   byte			 ck_a_p=0,ck_b_p=0;
 
   for(i=2;i<packet_len-2;i++)
    {
   		ck_a=ck_a + buffer[i];
   		ck_b=ck_b + ck_a;
   		}
   ck_a_p = buffer[packet_len-2];
   ck_b_p = buffer[packet_len-1];
   if ((ck_a_p==ck_a)& (ck_b_p==ck_b))
   	return    TRUE;
   else
 	return    FALSE;
}
